#include "odom.h"
#include <math.h>
#include <string.h>

const float wheel_diameter = 0.1; //100mm
#define WHEEL_GEAR_RATIO  0.074  //皮带1.5，减速器9，总共13.5的减速比1/13.5=0.074

Odometry_T Odometry = {
    .odom = {
        {0, 0, 0},
        {0, 0, 0}
    },
    .last_yaw = 0,
    .current_yaw = 0,
    .ms_tick = 0,
    .sum = odom_sum,
    .clear = odom_clear
};

/**
* @brief 从8个电机速度计算里程
* @param 8个电机的转速
*
*******************************
* |  [0]---[1]  |  [2]---[3]  |
* |  Wheel1     |  Wheel2     |
*******************************
*                             *
*******************************
* |  [4]---[5]  |  [6]---[7]  |
* |  Wheel3     |  Wheel4     |
*******************************
*                 X
*                 ^
*                 |
*                 |
*                 |
*  Y <------------O
*/


/**
* @brief rpm转速转换为米每秒
*/
float rpm2mps(short rpm)
{
    return (rpm/60.0*PI*wheel_diameter) * WHEEL_GEAR_RATIO;
}

/**
* @brief 米每秒转换为rpm, 最大速度1m/s
*/
short mps2rpm(float mps)
{
    if(mps<=1.0)
        return (short)(mps/WHEEL_GEAR_RATIO/(PI*wheel_diameter)*60);
    else
        return 0;
}

/**
* @brief 计算里程信息
* @param team_enable 是否是组队状态
*        motor_ 每个电机的设定速度
*        wheel_state 车轮状态，区分是否在自转
*        true_angles 每个回转支承的真实角度
*        keep_angles 每个回转支承的设定角度，可以由此判断是在斜向平移还是转弯
*/
void odom_sum(SetDoubleCar_T team_, struct MotorVels_T motor_, WheelState_T wheel_state, float* true_angles, float* keep_angles)
{
    u32 t = 0;
    int i=0;
    double average_vel = 0;
    double average_ang = 0;
    double distance = 0;
    double d_x = 0;
    double d_y = 0;
    double team_rx = team_.distance/2/1000.0;
    bool shifting = false; //是否在平移
    
    Odometry.current_yaw = Odometry.odom.pose.th/180.0*PI;
    t = Odometry.ms_tick;
    // 放在外面清零 Odometry.ms_tick = 0;
    for(i=0;i<8;i++)average_vel+=motor_.vel[i];
    for(i=0;i<4;i++)average_ang+=true_angles[i];
    average_vel = average_vel/8.0;
    /* 平均角度弧度 */
    average_ang = (average_ang/4.0)/180.0*PI;
    
    /* 组队情况下用中心速度计算 */
    if(team_.enable)
    {
        if(wheel_state==WHEEL_SPIN)
            distance = 0.0;
        else
            distance = (motor_.wheel_vel/60.0*PI*wheel_diameter) * WHEEL_GEAR_RATIO * (t/1000.0);
    }
    else
        distance = (average_vel/60.0*PI*wheel_diameter) * WHEEL_GEAR_RATIO * (t/1000.0);
    
    if( keep_angles[0] == keep_angles[1] &&
        keep_angles[1] == keep_angles[2] &&
        keep_angles[2] == keep_angles[3])
    {
        /* 平移状态 */
        shifting = true;
        d_x = distance * cos(average_ang + Odometry.current_yaw);
        d_y = distance * sin(average_ang + Odometry.current_yaw);
        //d_x = distance*cos(average_ang)*cos(current_th) - distance*sin(average_ang)*sin(current_th);
        //d_y = distance*cos(average_ang)*sin(current_th) + distance*sin(average_ang)*cos(current_th);
    }
    else
    {
        d_x = distance * cos(Odometry.current_yaw);
        d_y = distance * sin(Odometry.current_yaw);
    }
    
    if(team_.enable)
    {
        if(team_.position == TEAM_FRONT){
            Odometry.odom.pose.x = Odometry.odom.pose.x - team_rx*cos(Odometry.last_yaw) + d_x + team_rx*cos(Odometry.current_yaw);
            Odometry.odom.pose.y = Odometry.odom.pose.y - team_rx*sin(Odometry.last_yaw) + d_y + team_rx*sin(Odometry.current_yaw);
        }else if(team_.position == TEAM_BACK){
            Odometry.odom.pose.x = Odometry.odom.pose.x + team_rx*cos(Odometry.last_yaw) + d_x - team_rx*cos(Odometry.current_yaw);
            Odometry.odom.pose.y = Odometry.odom.pose.y + team_rx*sin(Odometry.last_yaw) + d_y - team_rx*sin(Odometry.current_yaw);
        }
    }
    else
    {
        Odometry.odom.pose.x += d_x;
        Odometry.odom.pose.y += d_y;
    }

    /* 以mm/s速度上传 */
    if(team_.enable)
    {
        if(shifting)
        {
            Odometry.odom.twist.vx = rpm2mps(motor_.wheel_vel * cos(keep_angles[0]))*1000;
            Odometry.odom.twist.vy = rpm2mps(motor_.wheel_vel * sin(keep_angles[0]))*1000;
        }
        else{
            Odometry.odom.twist.vx = rpm2mps(motor_.wheel_vel)*1000;
            Odometry.odom.twist.vy = 0;
        }
    }
    else
    {
        Odometry.odom.twist.vx = rpm2mps(average_vel * cos(average_ang))*1000;
        Odometry.odom.twist.vy = rpm2mps(average_vel * sin(average_ang))*1000;
    }
    
    Odometry.last_yaw = Odometry.current_yaw;
}

void odom_clear()
{
    memset(&Odometry.odom, 0, sizeof(Odometry.odom));
}

/* EOF */
